#include <ant_driver/base_control.h>

#define __NAME__ "base_control"

static float g_steering_gearRatio = 540.0/25.0;

BaseControl::BaseControl():
    allow_driverless_(false),
    canMsg_cmd1_valid_(false),
    canMsg_cmd2_valid_(false),
    stm32_brake_(0),
    last_set_speed_(0.0),
    emergency_brake_key_(false),
    driverless_keyon_(false),
    is_driverless_mode_(false),
    stm32_serial_port_(NULL),
    reconfig_brake_pid_(nullptr),
    state_lcm_publisher_(nullptr),
    speed_lcm_publisher_(nullptr),
    debug(__NAME__)
{	
    canMsg_cmd1.ID = ID_CMD_1;
    canMsg_cmd1.len = 8;
    canMsg_cmd1.type = Can2serial::STD_DATA_FRAME; //standard frame;
    memset(canMsg_cmd1.data, canMsg_cmd1.len, 0);

    canMsg_cmd2.ID = ID_CMD_2;
    canMsg_cmd2.len = 8;
    canMsg_cmd2.type = Can2serial::STD_DATA_FRAME;//standard frame;
    memset(canMsg_cmd2.data, canMsg_cmd1.len, 0);

    *(long *)canMsg_cmd2.data = 0;
    canMsg_cmd2.data[4] = 0xFF;
    canMsg_cmd2.data[5] = 0xFF; //set the steer angle value invalid

    emergencyBrakeFaultMsg_.sender = "base_control";
    emergencyBrakeFaultMsg_.timeout = 0;
    emergencyBrakeFaultMsg_.label = "emergencyBrake";

    speed_mean_filter_ = new dcom::MeanFilter(10);
    accel_mean_filter_ = new dcom::MeanFilter(10);
    expect_decel_filter_ = new dcom::MeanFilter(10);
    brake_aperture_filter_ = new dcom::MeanFilter(10);
}

BaseControl::~BaseControl(){
    stm32_serial_port_->close();
    if(stm32_serial_port_!=NULL){
        delete stm32_serial_port_;
        stm32_serial_port_ = NULL;
    }
    if(reconfig_brake_pid_){delete reconfig_brake_pid_;}
    if(state_lcm_publisher_){delete state_lcm_publisher_;}
    if(speed_lcm_publisher_){delete speed_lcm_publisher_;}
}

bool BaseControl::init(int argc,char**argv){
    ros::init(argc,argv,"base_control");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");

    nh_private.param<std::string>("obd_can_port_name", obd_can_port_name_, "");
    nh_private.param<std::string>("stm32_port_name", stm32_port_name_, "");
    nh_private.param<int>("stm32_baudrate",stm32_baudrate_,115200);
    assert(!obd_can_port_name_.empty() && !stm32_port_name_.empty());

    if(!dcom::loadVehicleParams(vehicle_params_, __NAME__)){
        return false;
    }

    nh_private.param<int>("steering_offset",steering_offset_,0);

    subscribers_.push_back(nh.subscribe("/vehicleCmdSet",1,&BaseControl::cmd_CB,this));
    state_pub_ = nh.advertise<driverless_common::VehicleState>("/vehicleStateSet",5);
    pub_faultIndication_ = nh.advertise<driverless_common::FaultIndication>("/fault_indication", 10);

    timer_ = nh.createTimer(ros::Duration(0.01), &BaseControl::timer10ms_CB, this);

    if(!openSerial(stm32_serial_port_,stm32_port_name_,stm32_baudrate_))
        return false;

    if(!can2serial.configure_port(obd_can_port_name_.c_str())){
        ROS_INFO("open port %s failed",obd_can_port_name_.c_str());
        return false;
    }
    else
        ROS_INFO("open port %s successfully",obd_can_port_name_.c_str());


    /*
    can2serial.clearCanFilter();

    can2serial.setCanFilter_alone(0x01,ID_STATE1); usleep(10000);
    can2serial.setCanFilter_alone(0x02,ID_STATE2); usleep(10000);
    can2serial.setCanFilter_alone(0x03,ID_STATE3); usleep(10000);
    can2serial.setCanFilter_alone(0x04,ID_STATE4); usleep(10000);

    can2serial.configBaudrate(500);
    */
    can2serial.StartReading();

    ROS_INFO("System initialization completed");

    state_lcm_publisher_ = new dcom::LcmCostomMsgPublisher();
    state_lcm_publisher_->init("base_control", "state", 9);

    speed_lcm_publisher_ = new dcom::LcmCostomMsgPublisher();
    speed_lcm_publisher_->init("base_control", "speed", 4);

    brake_pid_conf_.kp = 22.0;
    brake_pid_conf_.ki = 20;
    brake_pid_conf_.kd = 2.0;
    brake_pid_conf_.integral_saturation_high_ = 80.0;
    brake_pid_conf_.integral_saturation_low_ = -50.0;
    brake_pid_conf_.max_output = 100;
    brake_pid_conf_.max_interval = 0.5;
    brake_pid_conf_.min_output = 0.0;
    brake_pid_controller_.setPid(brake_pid_conf_);

    reconfig_brake_pid_ = new dcom::DynamicReconfigureServer("base_control", "brake_pid", 3);
    reconfig_brake_pid_->addParam(0, "kp", brake_pid_conf_.kp);
    reconfig_brake_pid_->addParam(1, "ki", brake_pid_conf_.ki);
    reconfig_brake_pid_->addParam(2, "kd", brake_pid_conf_.kd);
    reconfig_brake_pid_->registerCallback(&BaseControl::dynamicParamCallback, this);
    reconfig_brake_pid_->start();

    return true;
}

void BaseControl::dynamicParamCallback(const exlcm::DynamicReconfigureMsg *msg){
    if(msg->label == "brake_pid"){
        brake_pid_conf_.kp = reconfig_brake_pid_->getDouble(msg, 0);
        brake_pid_conf_.ki = reconfig_brake_pid_->getDouble(msg, 1);
        brake_pid_conf_.kd = reconfig_brake_pid_->getDouble(msg, 2);
        brake_pid_controller_.setPid(brake_pid_conf_);
        brake_pid_controller_.getPid().print();
    }
}

void BaseControl::run()
{
    readFromStm32_thread_ptr_ = boost::shared_ptr<boost::thread >
            (new boost::thread(boost::bind(&BaseControl::read_stm32_port, this)));

    boost::thread parse_msg(boost::bind(&BaseControl::parse_obdCanMsg,this));
}

void BaseControl::parse_obdCanMsg()
{
    CanMsg_t canMsg;

    while(ros::ok())
    {
        //ROS_INFO("parse_obdCanMsg  ing.....");
        usleep(3000);
        if(!can2serial.getCanMsg(canMsg))
            continue;

        std::lock_guard<std::mutex> lck(state_mutex_);
        switch(canMsg.ID){
        case ID_STATE1:{
            uint8_t gear = canMsg.data[0] >>4;
            if(gear == STATE_GEAR_DRIVE)
                stateSet_.gear = stateSet_.GEAR_DRIVE;
            else if(gear == STATE_GEAR_NEUTRAL)
                stateSet_.gear = stateSet_.GEAR_NEUTRAL;
            else if(gear == STATE_GEAR_REVERSE)
                stateSet_.gear = stateSet_.GEAR_REVERSE;
            else if(gear == STATE_GEAR_PARKING)
                stateSet_.gear = stateSet_.GEAR_PARKING;
            else
                stateSet_.gear = stateSet_.GEAR_INVALID;

            if((canMsg.data[2]&0x10))
                stateSet_.gear = stateSet_.GEAR_INVALID;

            stateSet_.accel_pedel_aperture = canMsg.data[1] * 0.4;
            stateSet_.brake_pedel_aperture = canMsg.data[2] & 0x01;
            if((canMsg.data[2] & 0x02))
                stateSet_.accel_pedel_aperture = 255;  //invalid
            if((canMsg.data[2] & 0x04))
                stateSet_.brake_pedel_aperture = 255;  //invalid

            stateSet_.ready = bool(canMsg.data[2]&0x20);
            stateSet_.driverless = bool(canMsg.data[2]&0x40);
            break;
        }
        case ID_STATE2:{
            bool wheel_speed_FL_valid = !(canMsg.data[0] >>6);
            float wheel_speed_FL = ((canMsg.data[0]&0x3f)*256+canMsg.data[1])*0.0625;
            bool wheel_speed_FR_valid = !(canMsg.data[1] >>6);
            float wheel_speed_FR = ((canMsg.data[2]&0x3f)*256+canMsg.data[3])*0.0625;
            bool wheel_speed_RL_valid = !(canMsg.data[4] >>6);
            float wheel_speed_RL = ((canMsg.data[4]&0x3f)*256+canMsg.data[5])*0.0625;
            bool wheel_speed_RR_valid = !(canMsg.data[6] >>6);
            float wheel_speed_RR = ((canMsg.data[6]&0x3f)*256+canMsg.data[7])*0.0625;
            {
                size_t i =0;
                float total_speed = 0.0;
                if(wheel_speed_FL_valid) {i++; total_speed += wheel_speed_FL;}
                if(wheel_speed_FR_valid) {i++; total_speed += wheel_speed_FR;}
                if(wheel_speed_RL_valid) {i++; total_speed += wheel_speed_RL;}
                if(wheel_speed_RR_valid) {i++; total_speed += wheel_speed_RR;}

                speed_lcm_publisher_->setData(0, "FL", wheel_speed_FL);
                speed_lcm_publisher_->setData(1, "FR", wheel_speed_FR);
                speed_lcm_publisher_->setData(2, "RL", wheel_speed_RL);
                speed_lcm_publisher_->setData(3, "RR", wheel_speed_RR);
                speed_lcm_publisher_->publish();

                float mean_speed = stateSet_.speed;
                if(i == 0){
                    stateSet_.speed_validity = false;
                    //					    stateSet_.speed = stateSet_.speed;
                }
                else{
                    mean_speed = total_speed / i; //km/h
                    stateSet_.speed_validity = true;
                }

                float filtered_spd = speed_mean_filter_->filter(mean_speed);
                float accel = accel_calculator_.update(filtered_spd);
                float filtered_accel = accel_mean_filter_->filter(accel);

                stateSet_.speed = filtered_spd;
                stateSet_.acceleration = filtered_accel;

                state_lcm_publisher_->setData(4, "cur_speed", stateSet_.speed);
                state_lcm_publisher_->setData(5, "flitered_speed", filtered_spd);
                state_lcm_publisher_->setData(6, "cur_accel", accel);
                state_lcm_publisher_->setData(7, "filtered_accel", filtered_accel);

            }
            break;
        }
        case ID_STATE3:{
            stateSet_.turnlight_l = bool(canMsg.data[1]&0x02);
            stateSet_.turnlight_r = bool(canMsg.data[1]&0x01);
            stateSet_.brake_light = bool(canMsg.data[2]&0x01);
            stateSet_.low_beam = bool(canMsg.data[1]&0x20);
            stateSet_.high_beam = bool(canMsg.data[1]&0x10);
            stateSet_.horn = bool(canMsg.data[2]&0x02);
            break;
        }
        case ID_STATE4:{
            stateSet_.driverless = is_driverless_mode_ = bool(canMsg.data[0]&0x01);
            stateSet_.roadwheel_angle = (1080.0-(canMsg.data[1]*256+canMsg.data[2])*0.1)/g_steering_gearRatio;
            stateSet_.roadwheel_angle_validity = !(canMsg.data[1] == 0xff && canMsg.data[2] == 0xff);
            stateSet_.manualctrl_detected = bool(canMsg.data[0]&0x02);

            // 检测到手动控制, 此处只置位,不复位
            if(stateSet_.manualctrl_detected)
                manualCtrlDetected_ = true;

            float steeringAngle_speed = canMsg.data[3]*4; // deg/s
            break;
        }
        default:
            break;
        }
    }
}

void BaseControl::timer10ms_CB(const ros::TimerEvent& event)
{
    static uint32_t cnt = 0;
    static bool ctrlCmdvalid = false;
    ++ cnt;
    if(cnt%5 == 0){ //50ms
        //send cmd to stm32
        send_to_stm32_buf[5] = stm32_brake_ & 0x7f;
        if(is_driverless_mode_) send_to_stm32_buf[5] |= 0x80;
        else send_to_stm32_buf[5] &= 0x7f;

        send_to_stm32_buf[7] = generateCheckNum(send_to_stm32_buf,8);
        stm32_serial_port_->write(send_to_stm32_buf,8);

        std::lock_guard<std::mutex> lck(state_mutex_);
        stateSet_.base_ready = driverless_keyon_ && stateSet_.ready;
        stateSet_.emergency_brake = emergency_brake_key_;
        state_pub_.publish(stateSet_);

        state_lcm_publisher_->publish();
    }

    if(cnt%30 == 0){ // 300ms, 有效性检测, 若检测间隔小于cmd消息间隔, 数据将被降频或停止发送
        ctrlCmdvalid = (canMsg_cmd1_valid_ && canMsg_cmd2_valid_);
        canMsg_cmd1_valid_ = canMsg_cmd2_valid_ = false;
    }

    if(cnt%5 == 1){ //50ms
        if(ctrlCmdvalid){
            std::lock_guard<std::mutex> lck(canMsg_cmd1_mutex_);
            can2serial.sendCanMsg(canMsg_cmd1);
        }
    }
    else{ // 10ms
        if(ctrlCmdvalid){
            std::lock_guard<std::mutex> lck(canMsg_cmd2_mutex_);
            can2serial.sendCanMsg(canMsg_cmd2);

            //      float speed = canMsg_cmd2.data[1] * MAX_SPEED / 150.0;
            //      float brake_val = canMsg_cmd2.data[2];
            //      float accel = canMsg_cmd2.data[3] / 50.0;

            //      if(speed > 30 || brake_val >=100 || canMsg_cmd2.data[3]!=0){
            //        std::cout << "------------------------------------------------"<< std::endl;
            //        ROS_INFO("can bus speed: %.2f, brake: %.2f, accel: %.2f", speed, brake_val, accel);
            //        can2serial.showCanMsg(canMsg_cmd2);
            //      }
        }
    }
}

void BaseControl::cmd_CB(const driverless_common::VehicleCtrlCmd::ConstPtr cmd)
{
    static double last_time = 0;
    double now_time = ros::Time::now().toSec();
    float dt = now_time - last_time;
    last_time = now_time;
    if(dt > 0.1){
        brake_pid_controller_.reset();
        //ROS_INFO("brake_pid_controller_.reset();");
        return;
    }

    state_mutex_.lock();
    float ego_spd = stateSet_.speed;
    float ego_spd_ms = ego_spd / 3.6;
    float ego_deceleration = -stateSet_.acceleration;
    static float last_set_steeringAngle = stateSet_.roadwheel_angle * g_steering_gearRatio;
    state_mutex_.unlock();

    uint8_t set_gear = CMD_GEAR_INITIAL;
    if(cmd->gear == cmd->GEAR_DRIVE) set_gear = CMD_GEAR_DRIVE;
    else if(cmd->gear == cmd->GEAR_REVERSE) set_gear = CMD_GEAR_REVERSE;
    else if(cmd->gear == cmd->GEAR_PARKING) set_gear = CMD_GEAR_PARKING;
    else if(cmd->gear == cmd->GEAR_NEUTRAL) set_gear = CMD_GEAR_NEUTRAL;

    // 当未处于自动驾驶状态时，同时置driverless=true gear=D/R, 将导致无法上档
    // 可以理解为，底层系统处于自动驾驶状态后，通过捕获上升沿进行换挡
    // 因此，当未处于自动驾驶状态时，无论命令如何，均发送空挡请求
    if(!stateSet_.driverless) set_gear = CMD_GEAR_NEUTRAL;

    float cmd_spd = cmd->speed > 0 ? cmd->speed : 0;
    float set_speed = cmd_spd;
    float set_deceleration = fabs(cmd->deceleration);
    if(dcom::isZeroFloat(set_deceleration)){
        expect_decel_filter_->reset();
    }else{
        set_deceleration = expect_decel_filter_->filter(set_deceleration);
    }

    if(cmd_spd > 10.0 && cmd_spd > ego_spd){
        // 限制车辆期望速度变化率, 防止车辆急加速
        //increment越大，加速度越大
        float increment = 5.0/(ego_spd/5+1);  //当前速度越低，加速越快
        float speed_diff = set_speed - ego_spd;
        if(speed_diff > increment){
            set_speed = ego_spd + increment;
        }
    }

    // 20220516
    //当前速度为30，速度控制请求降为25，20等非0时，车速不降反增，
    //通过限制请求指令变化率，暂时解决了该问题
    //实测车速为25km/h 30km/h 29km/h 请求值突变导致异常加速
    float cmd_speed_err = set_speed - last_set_speed_;
    if(cmd_speed_err < -0.1){
        set_speed = last_set_speed_ -0.1;
    }
    last_set_speed_ = set_speed;
    float brake_aperture = 0.0;
    if(cmd->brake_mode == cmd->BRAKE_MODE_DECELERATION){
        const float brake_pid_min_spd = 2.0;

        if(set_deceleration > 0.01){
            if(ego_spd > brake_pid_min_spd){
                brake_aperture = brake_pid_controller_.control(set_deceleration, ego_deceleration, dt);
            }else{
                brake_aperture = dcom::computeYby2ends(1.0, 30, 8.0, 100, set_deceleration);
            }
        }else{
            brake_pid_controller_.reset();
        }

        // 根据当前减速度对请求速度进行限制
        if(set_deceleration > 0){
            float tmp_speed = (ego_spd_ms - set_deceleration * vehicle_params_.speed_control_period)*3.6;
            if(tmp_speed < 0){
                tmp_speed = 0;
            }
            if(set_speed > tmp_speed){
                set_speed = tmp_speed;
            }
        }

        if(dcom::isZeroFloat(brake_aperture)){
            brake_aperture_filter_->reset();
        }else{
            brake_aperture = brake_aperture_filter_->filter(brake_aperture);
        }
    }else if(cmd->brake_mode == cmd->BRAKE_MODE_PEDAL_APERTURE){
        brake_aperture = cmd->brake_pedel_aperture;
    }else{
        std::cerr << "invalid brake mode!" << std::endl;
        brake_aperture = 50;
        set_speed = 0.0;
    }

    // 速度较低且请求速度为0时, 保持制动力防止溜车
    if(ego_spd < 0.5 && dcom::isZeroFloat((set_speed))){
        brake_aperture = dcom::max_int(brake_aperture, 30);
    }

    int brake_val = brake_aperture * 3; // 奇瑞电制动
    stm32_brake_ = brake_aperture;
    if(brake_val > 0 && ego_spd < 3.0) brake_val = 0; // 车速较低时,禁用电制动

    state_lcm_publisher_->setData(0, "ept_decel", -set_deceleration);
    state_lcm_publisher_->setData(1, "aperture", brake_aperture);
    state_lcm_publisher_->setData(2, "cmd_speed", cmd->speed);
    state_lcm_publisher_->setData(3, "can_speed", set_speed);
    state_lcm_publisher_->setData(8, "cmd_accel", cmd->acceleration);


    if(set_speed > MAX_SPEED-1){
        set_speed = MAX_SPEED-1;
    }else if(set_speed < 0){
        set_speed = 0;
    }
    if(brake_val > 99){// 不可超过99， 否则车辆可能溜车
        brake_val = 99;
    }

    canMsg_cmd2_mutex_.lock();
    canMsg_cmd2.resetData();
    canMsg_cmd2.data[0] |= set_gear&0x0f;
    canMsg_cmd2.data[1] = uint8_t(set_speed * 10 * 15.0 / MAX_SPEED);
    canMsg_cmd2.data[2] = brake_val;
    canMsg_cmd2.data[3] = uint8_t(cmd->acceleration *50);

    //  debug(2) << "cmd_spd: " << set_speed << ", ego_spd: " << ego_spd
    //           << ", cmd_decel: " << set_deceleration << ", eBrake: " << brake_val << std::endl;

    float current_set_steeringAngle = cmd->roadwheel_angle * g_steering_gearRatio;  // -540~540deg

    if(cmd->drive_mode == cmd->DRIVE_MODE_AUTO){
        if(current_set_steeringAngle>530.0) current_set_steeringAngle=530;
        else if(current_set_steeringAngle<-500.0) current_set_steeringAngle =-500.0;

        if(current_set_steeringAngle - last_set_steeringAngle > vehicle_params_.max_steering_speed * dt)
            current_set_steeringAngle = last_set_steeringAngle + vehicle_params_.max_steering_speed * dt;
        else if(current_set_steeringAngle - last_set_steeringAngle < -vehicle_params_.max_steering_speed * dt)
            current_set_steeringAngle = last_set_steeringAngle - vehicle_params_.max_steering_speed * dt;

        last_set_steeringAngle = current_set_steeringAngle;
    }

    uint16_t steeringAngle = 10800 - (current_set_steeringAngle*10 - steering_offset_) ;

    canMsg_cmd2.data[4] =  uint8_t(steeringAngle / 256);
    canMsg_cmd2.data[5] = uint8_t(steeringAngle % 256);

    if(cmd->emergency_brake) canMsg_cmd2.data[6] |= 0x10;
    canMsg_cmd2_valid_ = true;
    canMsg_cmd2_mutex_.unlock();

    // std::cout << "set_gear: " << int(set_gear) << std::endl;
    // std::cout << "cmd->driverless： " << int(cmd->driverless) <<"  " << int(allow_driverless_) << "  "
    // 			<< int(manualCtrlDetected_) << std::endl;
    canMsg_cmd1_mutex_.lock();
    canMsg_cmd1.resetData();
    if(cmd->driverless && allow_driverless_ && !manualCtrlDetected_){
        canMsg_cmd1.data[0] |= 0x01;  // 请求进入自动驾驶
    }else{
        canMsg_cmd1.data[0] &= 0xfe;  // 请求退出自动驾驶
    }

    if(cmd->hand_brake) canMsg_cmd1.data[0] |= 0x04;
    if(cmd->turnlight_r) canMsg_cmd1.data[1] |= 0x01;
    if(cmd->turnlight_l) canMsg_cmd1.data[1] |= 0x02;
    if(cmd->low_beam) canMsg_cmd1.data[1] |= 0x20;
    if(cmd->high_beam) canMsg_cmd1.data[1] |= 0x40;
    if(cmd->brake_light || brake_aperture > 10) canMsg_cmd1.data[2] |= 0x01;
    if(cmd->horn) canMsg_cmd1.data[2] |= 0x02;
    canMsg_cmd1_valid_ = true;
    canMsg_cmd1_mutex_.unlock();
}

uint8_t BaseControl::generateCheckNum(const void* voidPtr,size_t len)
{
    const uint8_t *ptr = (const uint8_t *)voidPtr;
    uint8_t sum=0;

    for(int i=2; i<len-1 ; i++)
        sum += ptr[i];
    return sum;
}

bool BaseControl::openSerial(serial::Serial* & port_ptr, std::string port_name,int baud_rate)
{
    try{
        port_ptr = new serial::Serial(port_name,baud_rate,serial::Timeout::simpleTimeout(10));

        if (!port_ptr->isOpen()){
            std::stringstream output;
            output << "Serial port: " << port_name << " failed to open." << std::endl;
            delete port_ptr;
            port_ptr = NULL;
            return false;
        }else{
            std::stringstream output;
            output << "Serial port: " << port_name << " opened successfully." << std::endl;
            std::cout << output.str() <<std::endl;
        }
        port_ptr->flush();
    }catch (std::exception &e){
        std::stringstream output;
        output << "Error  " << port_name << ": " << e.what();
        std::cout << output.str() <<std::endl;
        return false;
    }

    return true;
}


void BaseControl::read_stm32_port(){
    size_t len = 0;
    stm32_serial_port_->flushInput();

    while(ros::ok()){
        ros::Duration(0.02).sleep();
        try{
            len = stm32_serial_port_->read(stm32_data_buf, STM32_MAX_NOUT_SIZE);
            //ROS_INFO("read_stm32_port get %d bytes",len);
        }
        catch (std::exception &e){
            std::stringstream output;
            output << "Error reading from serial port: " << e.what();
            std::cout << output.str() <<std::endl;
        }
        if(len == 0) continue;

        //for(int i=0;i<len;i++)
        //	printf("%x\t",stm32_data_buf[i]);
        //std::cout << std::endl;

        Stm32BufferIncomingData(stm32_data_buf, len);
    }
}

void BaseControl::Stm32BufferIncomingData(unsigned char *message, unsigned int length){
    static int buffer_index = 0;
    static int bytes_remaining =0;

    // add incoming data to buffer
    for (unsigned int ii=0; ii<length; ii++)
    {// make sure bufIndex is not larger than buffer
        if(buffer_index >= STM32_MAX_PKG_BUF_LEN)
        {
            buffer_index = 0;
            printf("Overflowed receive buffer. Buffer cleared.");
        }
        switch(buffer_index)
        {
        case 0: //nothing
            if(message[ii]==Stm32MsgHeaderByte0){
                stm32_pkg_buf[buffer_index++] = message[ii];
            }
            bytes_remaining = 0;
            break;
        case 1:
            if(message[ii]==Stm32MsgHeaderByte1){
                stm32_pkg_buf[buffer_index++] = message[ii];
                bytes_remaining =2; //2 bytes pkgLen
            }
            else{
                buffer_index = 0;
                bytes_remaining = 0;
            }
            break;
        case 2:
        case 3:
            stm32_pkg_buf[buffer_index++]=message[ii];
            bytes_remaining --;
            if(bytes_remaining == 0){
                bytes_remaining = (stm32_pkg_buf[2] << 8) + stm32_pkg_buf[3] ;

                //根据实际发送的包长最大小值进行限定(多重数据正确保障)
                if(bytes_remaining > 9 || bytes_remaining < 2)
                {
                    buffer_index = 0;
                    break;
                }
            }
            break;
        default:
            stm32_pkg_buf[buffer_index++] = message[ii];
            bytes_remaining --;
            if(bytes_remaining == 0){
                buffer_index = 0;
                parse_stm32_msgs();
            }
            break;
        }
    }// end for
}

void BaseControl::parse_stm32_msgs()
{
    const stm32MsgHeader* header = (const stm32MsgHeader*)stm32_pkg_buf;
    int data_len = ntohs(header->pkgLen);
    if(stm32_pkg_buf[data_len+3] != generateCheckNum(stm32_pkg_buf,data_len+4))
        return ;
    // std::cout << "pkg id: " << int(header->id) << std::endl;
    if(header->id == 0x01){
        const stm32Msg1_t* msg1 = (const stm32Msg1_t*)stm32_pkg_buf;

        static bool last_allow_driverless = false;
        if(emergency_brake_key_ && !msg1->is_emergency_brake){
            emergencyBrakeFaultMsg_.fault = false;
            pub_faultIndication_.publish(emergencyBrakeFaultMsg_);
        }else if(!emergency_brake_key_ && msg1->is_emergency_brake){
            emergencyBrakeFaultMsg_.fault = true;
            pub_faultIndication_.publish(emergencyBrakeFaultMsg_);
        }

        emergency_brake_key_ = msg1->is_emergency_brake;
        driverless_keyon_ = msg1->is_start; // 自动驾驶按钮状态
        allow_driverless_ = (driverless_keyon_ && !emergency_brake_key_ && stateSet_.ready);

        if(last_allow_driverless == false && allow_driverless_)
            manualCtrlDetected_ = false; //重新允许自动驾驶，清除历史检测到的人工介入标志

        last_allow_driverless = allow_driverless_;

        // std::cout << "allow_driverless: " << allow_driverless_ << std::endl
        // 		<< "s_start: " << int(msg1->is_start) << "\nis_emergency_brake:" <<  int(msg1->is_emergency_brake)
        // 		<< std::endl;;
    }

}



int main(int argc,char**argv)
{
    BaseControl base_control;

    if(base_control.init(argc,argv)){
        base_control.run();
        ros::spin();
    }

    printf("base_control_node has exited");
    return 0;
}












